/*
 * @Author: qsy
 * @Date: 2024-12-28 14:39:05
 * @LastEditors: qsy
 * @LastEditTime: 2024-12-28 17:00:16
 * @Description: 头文件
 */
#include "board_adxl345.h"


#include <stdio.h>
#include <stdbool.h>
#include <unistd.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "math.h"

// 角度转弧度
#define DEG_TO_RAD(x) ((x) * M_PI / 180.0f)

static float convert_raw_to_g(short raw_data) {
    return (raw_data * 0.0039f * 9.8f);  // 转换为 m/s²
}

static void calibrate_orientation(float *pitch, float *roll, float *offset_x, float *offset_y, float *offset_z) {
    short x, y, z;
    float sum_x = 0, sum_y = 0, sum_z = 0;
    
    // 多次采样取平均，提高校准精度
    printf("正在进行方向校准，请保持设备静止...\n");
    for(int i = 0; i < 100; i++) {
        ADXL345_ReadAverage(&x, &y, &z, 10);
        sum_x += convert_raw_to_g(x);
        sum_y += convert_raw_to_g(y);
        sum_z += convert_raw_to_g(z);
        vTaskDelay(10 / portTICK_PERIOD_MS);
    }
    
    // 计算平均值作为偏移量
    *offset_x = sum_x / 100.0f;
    *offset_y = sum_y / 100.0f;
    *offset_z = sum_z / 100.0f;
    
    // 计算俯仰角和横滚角
    *pitch = atan2(-(*offset_x), sqrt((*offset_y) * (*offset_y) + (*offset_z) * (*offset_z)));
    *roll = atan2(*offset_y, *offset_z);
    
    printf("初始方向校准完成:\n");
    printf("静止状态加速度 - X: %.4f Y: %.4f Z: %.4f\n", *offset_x, *offset_y, *offset_z);
    printf("俯仰角: %.2f°\n横滚角: %.2f°\n", 
           *pitch * 180.0f / M_PI, 
           *roll * 180.0f / M_PI);
}

static void transform_acceleration(float acc_x, float acc_y, float acc_z,
                                float pitch, float roll,
                                float offset_x, float offset_y, float offset_z,
                                float *real_x, float *real_y, float *real_z) {
    // 减去静态偏移
    acc_x -= offset_x;
    acc_y -= offset_y;
    acc_z -= offset_z;
    
    // 创建旋转矩阵
    float cos_pitch = cosf(pitch);
    float sin_pitch = sinf(pitch);
    float cos_roll = cosf(roll);
    float sin_roll = sinf(roll);
    
    // 应用旋转矩阵转换坐标系
    *real_x = acc_x * cos_pitch + acc_z * sin_pitch;
    *real_y = acc_x * sin_roll * sin_pitch + acc_y * cos_roll - acc_z * sin_roll * cos_pitch;
    *real_z = -acc_x * cos_roll * sin_pitch + acc_y * sin_roll + acc_z * cos_roll * cos_pitch;
}

static void monitor_task(void *arg)
{
    float pitch, roll;
    float offset_x, offset_y, offset_z;
    
    // 首先进行方向校准
    calibrate_orientation(&pitch, &roll, &offset_x, &offset_y, &offset_z);
    printf("校准完成，开始测量...\n");

    while(1)
    {
        short x_value, y_value, z_value;
        float acc_x, acc_y, acc_z;
        float real_acc_x, real_acc_y, real_acc_z;
        float total_acc;

        // 读取当前加速度值并多次平均
        ADXL345_ReadAverage(&x_value, &y_value, &z_value, 20);
        
        // 转换为实际加速度值
        acc_x = convert_raw_to_g(x_value);
        acc_y = convert_raw_to_g(y_value);
        acc_z = convert_raw_to_g(z_value);

        // 转换到标准坐标系并去除重力影响
        transform_acceleration(acc_x, acc_y, acc_z,
                             pitch, roll,
                             offset_x, offset_y, offset_z,
                             &real_acc_x, &real_acc_y, &real_acc_z);

        // 计算合加速度
        total_acc = sqrt(real_acc_x * real_acc_x + 
                        real_acc_y * real_acc_y + 
                        real_acc_z * real_acc_z);

        // 打印实时运动加速度数据
        printf("\n实际运动加速度(m/s²):\n");
        printf("X: %.4f  Y: %.4f  Z: %.4f\n", real_acc_x, real_acc_y, real_acc_z);
        printf("合加速度: %.4f\n", total_acc);

        // 显示显著运动（降低阈值）
        if(total_acc > 0.5f) {
            printf("检测到运动！\n");
        }

        vTaskDelay(500 / portTICK_PERIOD_MS);
    }
}



void app_main(void)
{
    ADXL345_Init();

    /*-------------------------- 创建线程 ---------------------------*/
    xTaskCreate(monitor_task, "monitor_task", 1024*10, NULL, 2, NULL);
}

